%%动力学模型数据
m = 1580;%整车质量
l = 2.66;%轴距
a = 1.05;%质心到前轴距离
b = l - a;%质心到后轴距离
Iz = 2059.2;%绕ｚ轴转动惯量
r = 0.353;%车轮有效滚动半径
Cf = -75000;%前轮轮胎侧偏刚度
Cr = -68000;%后轮轮胎侧偏刚度
Pm = 180;%电机最大功率
Tm = 380;%电机最大扭矩
vx = 50/15;%纵向速度，这个后面需要调整

A = [(Cf+Cr)/(m*vx) (a*Cf-b*Cr)/(m*vx)-vx ; (a*Cf-b*Cr)/(Iz*vx) (a*a*Cf+b*b*Cr)/(Iz*vx)];%车A阵
B = [-Cf/m ; -a*Cf/Iz];%车B阵


%% 离散轨迹点
T = 15; % 仿真时间
%低速工况
%x0 = 0; xT = 50; % 纵向起终点位置
%y0 = 0; yT = 5;  % 横向起终点位置
%vx0 = 0; vxT = 0; % 纵向起终点速度
%ax0 = 0; axT = 0; % 纵向起终点加速度
%中速工况
x0 = 0; xT = 200; % 纵向起终点位置
y0 = 0; yT = 10;  % 横向起终点位置
vx0 = 10; vxT = 15; % 纵向起终点速度
ax0 = 0; axT = 0; % 纵向起终点加速度
%高速工况
%x0 = 0; xT = 350; % 纵向起终点位置
%y0 = 0; yT = 15;  % 横向起终点位置
%vx0 = 20; vxT = 25; % 纵向起终点速度
%ax0 = 0; axT = 0; % 纵向起终点加速度

A_x = [1,  0,    0,      0,        0,         0;
       0,  1,    0,      0,        0,         0;
       0,  0,    2,      0,        0,         0;
       1,  T,    T^2,    T^3,      T^4,       T^5;
       0,  1,    2*T,    3*T^2,    4*T^3,     5*T^4;
       0,  0,    2,      6*T,      12*T^2,    20*T^3];
b_x = [x0; vx0; ax0; xT; vxT; axT];
coeff_x = A_x \ b_x; % 纵向多项式系数[a0,a1,...,a5]

A_y = A_x;
b_y = [y0; 0; 0; yT; 0; 0];
coeff_y = A_y \ b_y; % 横向多项式系数[b0,b1,...,b5]

% 生成离散轨迹点
t = 0:0.1:T;
x_ref = coeff_x(1) + coeff_x(2)*t + coeff_x(3)*t.^2 + ...
        coeff_x(4)*t.^3 + coeff_x(5)*t.^4 + coeff_x(6)*t.^5;
    
y_ref = coeff_y(1) + coeff_y(2)*t + coeff_y(3)*t.^2 + ...
        coeff_y(4)*t.^3 + coeff_y(5)*t.^4 + coeff_y(6)*t.^5;

dx = coeff_x(2) + 2*coeff_x(3)*t + 3*coeff_x(4)*t.^2 + ...
     4*coeff_x(5)*t.^3 + 5*coeff_x(6)*t.^4;
dy = coeff_y(2) + 2*coeff_y(3)*t + 3*coeff_y(4)*t.^2 + ...
     4*coeff_y(5)*t.^3 + 5*coeff_y(6)*t.^4;
ddx = 2*coeff_x(3) + 6*coeff_x(4)*t + 12*coeff_x(5)*t.^2 + 20*coeff_x(6)*t.^3;
ddy = 2*coeff_y(3) + 6*coeff_y(4)*t + 12*coeff_y(5)*t.^2 + 20*coeff_y(6)*t.^3;

theta_ref = atan2(dy, dx); % 航向角
k_ref = (dx.*ddy - dy.*ddx) ./ (dx.^2 + dy.^2).^(3/2); % 曲率

%plot(t, x_ref, 'b-', 'LineWidth', 1.5, 'DisplayName', '期望纵向轨迹');
%hold on;
%plot(out.y4.Time, out.y5.data, 'r--', 'LineWidth', 1.5, 'DisplayName', '实际纵向轨迹');
%plot(t, y_ref, 'r-', 'LineWidth', 2, 'DisplayName', '期望横向轨迹'); 
%hold on;
%plot(out.y4.Time, out.y4.data, 'g--', 'LineWidth', 1.5, 'DisplayName', '实际横向轨迹');
%text(11.5,13.1,' 中速');
%text(pi/2,0,' \leftarrow 余弦');
%title( '横向轨迹跟踪' )

%% LQR
%状态空间矩阵
A1 = [0, 1, 0, 0;
     0, (Cf+Cr)/(m*vx), -(Cf+Cr)/m, (a*Cf-b*Cr)/(m*vx);
     0, 0, 0, 1;
     0, (a*Cf-b*Cr)/(Iz*vx), -(a*Cf-b*Cr)/Iz, (a^2*Cf+b^2*Cr)/(Iz*vx)];

B1 = [0; -Cf/m; 0; -a*Cf/Iz];
C1 = [0; (a*Cf-b*Cr)/(m*vx) - vx; 0; (a^2*Cf+b^2*Cr)/(Iz*vx)];

%LQR权重矩阵
Q = diag([30, 1, 5, 1]);  % 状态权重 [ey, ey_dot, ephi, ephi_dot]
R = 10;                   % 控制权重 [delta]

%求解Riccati方程得到反馈矩阵K
[K, ~, ~] = lqr(A1, B1, Q, R); 